//#include "InverseKin_Ira.h"
#include "iostream"
#include "newmatap.h"     // newmat advanced functions
#include "newmatio.h"     // newmat headers including output functions
#include <math.h>
#include <stdexcept>
#include "utils2.h"
#include "robot2.h"
#include <stdio.h>

using namespace std;
//*************************************************************************
              //         inv_tmatrix(t)
//*************************************************************************
extern "C" ReturnMatrix inv_tmatrix(const Matrix& t)
{
        SquareMatrix tinv = IdentityMatrix(4);
        tinv.submatrix(1,3,1,3) = t.submatrix(1,3,1,3).t();
        tinv.submatrix(1,3,4,4) = -tinv.submatrix(1,3,1,3) * t.submatrix(1,3,4,4);
        tinv.release();
        return tinv;
}

//*************************************************************************
              //         inv_kin_pa10_all
//*************************************************************************
extern "C" ReturnMatrix inv_kin_pa10_all(const Matrix & Tobj, ColumnVector& good_soln, int* nGoodSolns)
//ReturnMatrix inv_kin_pa10_all()
{

    Real fourbyfourident[] = {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0};
    Real p2[] = {0.0,0.0,0.0};
    Matrix RotBase(3,3);

   // load values row by row
   /*
   RotBase.row(1) <<  cos(M_PI/2) << -1*sin(M_PI/2) <<  0.0;
   RotBase.row(2) <<  sin(M_PI/2) <<  cos(M_PI/2) <<  0.0;
   RotBase.row(3) <<  0.0 <<  0.0 <<  1.0; */

   RotBase.row(1) <<  1.0 << 0.0 <<  0.0;
   RotBase.row(2) <<  0.0 <<  1.0 <<  0.0;
   RotBase.row(3) <<  0.0 <<  0.0 <<  1.0;


        Matrix T_f_base(4,4);

        T_f_base << fourbyfourident;
        T_f_base.submatrix(1,3,1,3) << RotBase;
        T_f_base.submatrix(1,3,4,4) << p2;
        SquareMatrix T_1_tool = inv_tmatrix(T_f_base)*Tobj;
        Matrix angles(6,8);

        //* --------------------------------------------------------------------------
        // * Step 1. Extract DH parameters from robot object
        // * -------------------------------------------------------------------------
        double alp12 = M_PI/2;
        double a23 = 0.450;
        double alp23 = 0;
        double alp34 = M_PI/2;
        double alp45 = M_PI/2;
        double S4 = 0.480;
        double alp56 = M_PI/2;
        double alp67 = M_PI/2; //?? I remember this being some sort of hack...
        //double alp67 = 0.0;
        double S6 = 0.070;

        // Use the constant mechanism parameters to find cj, sj, cij, and sij.
        double c12 = cos(alp12); double s12 = sin(alp12);
        double c23 = cos(alp23); double s23 = sin(alp23);
        double c34 = cos(alp34); double s34 = sin(alp34);
        double c45 = cos(alp45); double s45 = sin(alp45);
        double c56 = cos(alp56); double s56 = sin(alp56);
        double c67 = cos(alp67); double s67 = sin(alp67);

        // Check passed in values... derivation of the analytic solution below
        // depeneds on a few assumptions!
        if ( s23 > 1e-9 ) throw out_of_range("Alpha_23 should be equal to 0 degrees.");
        if ( c34 > 1e-9 ) throw out_of_range("Alpha_34 should be equal to 90 degrees.");
       // --------------------------------------------------------------------------
         // Step 2. Create a virtual closed-loop mechanism so that we can write a
         // vector loop equation.
         //
         //   Vector-loop equation for PA-10:
         //   S1*S1_vec + a23*a23_vec + S4*S4_vec + S6*S6_vec + S7*S7_vec + a71*a71_vec = 0_vec
       // ------------------------------------------------------------------------
        ClosedLoopParams p;
        //SquareMatrix T_1_6 = T_1_tool * inv_tmatrix(this->T_6_tool);
        //close_loop(T_1_tool, T_1_6, T_f_base, this->links[1].get_d(), p);
        close_loop(T_1_tool, T_1_tool, T_f_base, 0.0, p);

        // --------------------------------------------------------------------------
         // Step 3. Solve for theta_1 and phi_1
         //
         //   There should be two valid solutions for theta_1 (phi_1). Therefore this
         //   creates the first branch in our tree of solutions.
         // ------------------------------------------------------------------------
        double c7 = cos(p.th7); double s7 = sin(p.th7);
        double c71 = cos(p.alp71); double s71 = sin(p.alp71);
        double X7 = s67*s7;
        double Y7 = -(s71*c67 + c71*s67*c7);
        double Z7 = c71*c67 - s71*s67*c7;
        // Expand z-component of vector loop eqn and collect s1 and c1 terms to get
        //   A*c1 + B*s1 + D = 0
        double A = -S6*Y7 + p.S7*s71;
        double B = -S6*X7 - p.a71;
        double D = p.S1*c12; // Should be equal to zero
        double ang1 = 0, ang2 = 0;

        // If the does not work then there is no point in continuing.
        if (!solve_trig(A, B, D, ang1, ang2)) throw out_of_range("Encountered bad solutions at theta_1!");

        ColumnVector th1(8);
        th1.submatrix(1,4,1,1) = ang1; th1.submatrix(5,8,1,1) = ang2;
        good_soln = 1.0;

        ang1 = ang1 - p.gam1; ang2 = ang2 - p.gam1;
        if ( !fix_angle_pa10(1,ang1) ) good_soln.submatrix(1,4,1,1) = 0;
        if ( !fix_angle_pa10(1,ang2) ) good_soln.submatrix(5,8,1,1) = 0;
        angles.submatrix(1,1,1,4) = ang1;
        angles.submatrix(1,1,5,8) = ang2;

        //* -------------------------------------------------------------------------
        //Solutions for the rest of the joint angles
        //The rest of this program consists of a various number of nested FOR and
        //IF statements. The purpose of this code is to traverse the entire
        //solution tree.
        //
         //   The if statments are used to primarily set and check the status of the
         //   good_soln array. If there is a problem calculating any of the joint
         //   angles at any part of the tree the problematic joint angle will set the
         //   appropriate sections of good_soln = 0 which will essentially cancel
         //   calculation of the tree from that angle forward.
         // ------------------------------------------------------------------------
        for (int th1index = 1; th1index <= 5; th1index=th1index+4)  //[1, 5]
        {
                //--------------------------------------------------------------------------
                 // Step 4. Solve for theta_3
                 //
                 //   There are two possible solutions for theta_3. Thus creating the second
                 //   branch in the tree of solutions.
                 //------------------------------------------------------------------------
                double c1 = cos(th1(th1index)); double s1 = sin(th1(th1index));
                double X1 = s71*s1;
                double Y1 = -(s12*c71 + c12*s71*c1);
                double X71 = X7*c1 - Y7*s1;
                double Y71 = c12*(X7*s1 + Y7*c1) - s12*Z7;
                double Z71 = s12*(X7*s1 + Y7*c1) + c12*Z7;
                A = -S6*X71 - p.S7*X1 - p.a71*c1;
                B = -p.S1*s12 + S6*Y71 + p.S7*Y1 + p.a71*s1*c12;
                double s3 = (pow(A,2) + pow(B,2) - pow(a23,2) - pow(S4,2))/(2*S4*a23);

                if ( isabout(fabs(s3), 1, 1e-6) ) s3=1.0;

                if (fabs(s3) > 1.0) {
                        good_soln.submatrix(th1index,th1index+3,1,1) = 0;
                        continue;
                }

                asin2(s3, ang1, ang2);
                if ( !fix_angle_pa10(3,ang1) ) good_soln.submatrix(th1index,th1index + 1,1,1) = 0;
                if ( !fix_angle_pa10(3,ang2) ) good_soln.submatrix(th1index+2,th1index+3,1,1) = 0;
                angles.submatrix(3,3,th1index,th1index+1) = ang1;
                angles.submatrix(3,3,th1index+2,th1index+3) = ang2;

                for (int th3index = th1index; th3index <= th1index+2; th3index = th3index + 2)
                {
                        //----------------------------------------------------------------------
                        //Step 5. Solve for theta_2
                        //
                        //There is one unique solution for theta_2.
                        //--------------------------------------------------------------------
                        double c3 = cos(angles(3,th3index));
                        SquareMatrix M(2); ColumnVector b(2);
                        M(1,1) = a23 + S4*s3;   M(1,2) = S4*c3;
                        M(2,1) = -S4*c3;        M(2,2) = a23 + S4*s3;
                        b(1) = -S6*X71 - p.S7*X1 - p.a71*c1;
                        b(2) = -p.S1*s12 + S6*Y71 + p.S7*Y1 + p.a71*s1*c12;
                        ColumnVector x;

                        if ( !solve_pair(M,b,x) ) {
                                good_soln.submatrix(th3index,th3index+1,1,1) = 0;
                                continue;
                        }

                        double c2 = x(1); double s2 = x(2);
                        ang1 = atan2(s2,c2);
                        if ( !fix_angle_pa10(2,ang1) ) good_soln.submatrix(th3index,th3index+1,1,1) = 0;
                        angles.submatrix(2,2,th3index,th3index+1) = ang1;

                        //*----------------------------------------------------------------------
                        // * Step 6. Solve for theta_5
                        // *
                        // *   There are two possible solutions for theta_5 for each value of
                        // *   theta_3. This creates the second branch in the tree of solutions.
                        // *--------------------------------------------------------------------
                        double X712 = X71*c2 - Y71*s2;
                        double Y712 = c23*(X71*s2 + Y71*c2) - s23*Z71;
                        double Z712 = s23*(X71*s2 + Y71*c2) + c23*Z71;
                        double X7123 = X712*c3 - Y712*s3;
                        double Y7123 = c34*(X712*s3 + Y712*c3) - s34*Z712;
                        double Z7123 = s34*(X712*s3 + Y712*c3) + c34*Z712;
                        double c5 = -(Z7123 - c45*c56)/(s45*s56);

                        if ( isabout( fabs(c5), 1, 1e-6 ) ) c5 = 1.0;

                        if (fabs(c5) > 1.0) {
                                good_soln.submatrix(th3index,th3index+1,1,1) = 0;
                                continue;
                        }

                        acos2(c5, ang1, ang2);
                        if ( !fix_angle_pa10(5,ang1) ) good_soln(th3index) = 0;
                        if ( !fix_angle_pa10(5,ang2) ) good_soln(th3index+1) = 0;
                        angles(5,th3index) = ang1;
                        angles(5,th3index+1) = ang2;

                        for (int th5index = th3index; th5index <= th3index+1; th5index++)
                        {
                                //------------------------------------------------------------------
                                // * Step 7. Solve for theta_4
                                // *
                                // *   There is one unique solution for theta_4.
                                // *----------------------------------------------------------------
                                double s5 = sin(angles(5,th5index));
                                double X5_bar = s56*s5;
                                double Y5_bar = -(s45*c56 + c45*s56*c5);
                                M(1,1) = X5_bar;    M(1,2) = -Y5_bar;
                                M(2,1) = Y5_bar;    M(2,2) = X5_bar;
                                b(1) = X7123;
                                b(2) = -Y7123;

                                if ( !solve_pair(M,b,x) ) {
                                        good_soln(th5index) = 0;
                                        continue;
                                }

                                double c4 = x(1); double s4 = x(2);
                                ang1 = atan2(s4,c4);
                                if ( !fix_angle_pa10(4,ang1) ) good_soln(th5index) = 0;
                                angles(4,th5index) = ang1;

                                //------------------------------------------------------------------
                                //Step 8. Solve for theta_6
                                //
                                //There is one unique solution for theta_6.
                                //----------------------------------------------------------------
                                double bX4 = s45*s4;
                                double bY4 = -(s34*c45 + c34*s45*c4);
                                double bZ4 = c34*c45 - s34*s45*c4;
                                double X43 = bX4*c3 - bY4*s3;
                                double Y43 = c23*(bX4*s3 + bY4*c3) - s23*bZ4;
                                double Z43 = s23*(bX4*s3 + bY4*c3) + c23*bZ4;
                                double X432 = X43*c2 - Y43*s2;
                                double Y432 = c12*(X43*s2 + Y43*c2) - s12*Z43;
                                double Z432 = s12*(X43*s2 + Y43*c2) + c12*Z43;
                                double X4321 = X432*c1 - Y432*s1;
                                double Y4321 = c71*(X432*s1 + Y432*c1) - s71*Z432;
                                double Z4321 = s71*(X432*s1 + Y432*c1) + c71*Z432;
                                double X43217 = X4321*c7 - Y4321*s7;
                                double Y43217 = c67*(X4321*s7 + Y4321*c7) - s67*Z4321;
                                double c6 = Y43217/s56;
                                double s6 = X43217/s56;

                                ang1 = atan2(s6,c6);
                                if ( !fix_angle_pa10(6,ang1) ) {
                                        good_soln(th5index) = 0;
                                }
                                angles(6,th5index) = ang1;
                        } // END THETA 5 BRANCH
                } // END THETA 3 BRANCH
        } // END THETA 1 BRANCH

        // Add in joint offsets
        //angles.row(2) = angles.row(2) - this->links[2].get_joint_offset();
        //angles.row(3) = angles.row(3) - this->links[3].get_joint_offset();
        angles.row(1) = angles.row(1) - M_PI/2;
        angles.row(2) = angles.row(2) - M_PI/2;
        angles.row(3) = angles.row(3) - M_PI/2;
        angles.row(4) = angles.row(4) - M_PI;
        angles.row(5) = angles.row(5) - M_PI;
        angles.row(6) = angles.row(6) - 0.0;
        for (int n=1; n <= 8; ++n) {
            if(good_soln(n)) {
                (*nGoodSolns)++;
            }
        }
        angles.release();
        return angles;
}
/* ///////////////////////////////////////////////////////////
                Main Function Deleted
//////////////////////////////////////////////////////////  */

extern "C" int AddVxIra(const int a, const int b)
{
    return a+b;
}

extern "C" int InvKinIra(double* desiredTransform, double* jointAngles, int* sols)
{
ColumnVector good_soln(8), qout(6), result(6);
int numGoodSolutions = -1, cnt = 0;
Matrix Tobj(4,4);
Tobj << desiredTransform;
qout = 0.0;
        Matrix Q = inv_kin_pa10_all(Tobj, good_soln, &numGoodSolutions);
        if (numGoodSolutions > 0) {
                for (int n=1; n <= 8; ++n) {
                        if (good_soln(n)) {
                            result = Q.submatrix(1,6,n,n)*180/M_PI;
                                for (int jj=1; jj <=6; ++jj) {
                                    jointAngles[cnt+jj-1] = result(jj);
                                }
                        }
                        cnt = cnt+6;
                }
                return 0;
        } else {
                 return 1;
        }
}
